/**
 * @Author       : Mo 2427995270@qq.com
 * @Date         : 2025-01-23 10:04:36
 * @LastEditTime : 2025-07-25 17:07:26
 * @FilePath     : \KT-02DP-APP\libs\modbus.c
 * @Description  : modbus library
 * @Version      : V1.0
 * @History      :
 * @Note         : 依赖libcrc库 crc字节序为小端 寄存器值为大端
 */
/***************************************Includes***********************************/
#include "modbus.h"
#include "checksum.h"
#include "hal_delay.h"

/************************************Private Macros********************************/
#define LOCK_TIMEOUT 10000

#if USE_OS == OS_FREERTOS
#define DELAY_MS(ms) delay_ms_os(ms)
#else
#define DELAY_MS(ms) delay_ms_not_os(ms)
#endif

#define FUN_CODE_HOLD_REGS 0x03
#define FUN_CODE_INPUT_REGS 0x04
#define FUN_CODE_WRITE_REG 0X06
#define FUN_CODE_WRITE_REGS 0x10

/**********************************Private Statement*******************************/

static int modbus_client_analyze_03(modbus_dev_t *dev, uint16_t *reg_data, uint16_t reg_num);
static int modbus_client_analyze_06(modbus_dev_t *dev, uint16_t reg_data);
static int modbus_client_analyze_10(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_num);
static int modbus_server_analyze_03_04(modbus_bus_t *bus, uint8_t fun, uint16_t reg_addr, uint16_t reg_num);
static int modbus_server_analyze_06(modbus_bus_t *bus, uint16_t reg_addr, uint16_t reg_data);
static int modbus_server_analyze_10(modbus_bus_t *bus, uint16_t reg_addr, uint16_t reg_num);
static int modbus_client_03_04_fun(modbus_dev_t *dev, uint8_t fun, uint16_t reg_addr, uint16_t reg_num, uint16_t *reg_data);
static void modbus_client_fill_head(uint8_t *data, uint8_t addr, uint8_t fun, uint16_t reg_addr, uint16_t reg_data);
static int modbus_send(modbus_dev_t *dev, uint8_t *data, uint16_t len);
static int modbus_verify(modbus_dev_t *dev, uint8_t fun_code);
static void modbus_clear_err(modbus_dev_t *dev);
static int modbus_check_err(modbus_dev_t *dev);
static void modbus_lock(modbus_bus_t *bus);
static void modbus_unlock(modbus_bus_t *bus);

/***********************************Public Variables*******************************/



/**********************************Private Variables*******************************/



/**********************************Public Functions********************************/
/**
 * @brief modbus客户端初始化
 * @param  bus
 * @param  send_cb          发送数据回调函数
 * @param  user_data        用户数据
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_client_init(modbus_bus_t *bus, modbus_send_cb_t send_cb, void *user_data) {
    // 服务器地址为0时，为客户端(主机)
    bus->server_addr = 0;
    bus->send_cb = send_cb;
    bus->data = NULL;
    bus->len = 0;
    bus->user_data = user_data;

#if USE_OS == OS_FREERTOS
    bus->mutex = xSemaphoreCreateMutex();
    modbus_unlock(bus);
#endif
    return 0;
}
/**
 * @brief modbus服务端初始化
 * @param  bus
 * @param  server_addr      监听地址
 * @param  send_cb          发送数据回调函数
 * @param  read_reg_cb      读取寄存器回调函数
 * @param  write_reg_cb     写入寄存器回调函数
 * @param  user_data        用户数据
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_server_init(modbus_bus_t *bus, uint8_t server_addr, modbus_send_cb_t send_cb, void *user_data) {
    if (server_addr == 0) {
        return -1;
    }
    bus->server_addr = server_addr;
    bus->send_cb = send_cb;
    bus->user_data = user_data;
    return 0;
}
/**
 * @brief modbus添加设备
 * @param  bus              modbus总线
 * @param  dev              modbus设备
 * @param  dev_addr         设备地址
 * @param  timeout          超时时间 ms
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_client_add_dev(modbus_bus_t *bus, modbus_dev_t *dev, uint8_t dev_addr, uint16_t timeout) {
    if (bus->server_addr) {
        return -1; // 不能在服务端添加设备
    }
    dev->bus = bus;
    dev->dev_addr = dev_addr;
    dev->timeout = timeout;
    return 0;
}

/**
 * @brief modbus添加读写控制回调函数
 * @param  bus
 * @param  ctrl_cb
 * @param  user_data        用户数据
 * @note null
 */
void modbus_add_ctrl_cb(modbus_bus_t *bus, modbus_ctrl_cb_t ctrl_cb, void *user_data) {
    bus->ctrl_cb = ctrl_cb;
    bus->user_data_ctrl = user_data;
    if (ctrl_cb) {
        ctrl_cb('r', user_data);
    }
}
/**
 * @brief 服务端添加读保持寄存器回调
 * @param  bus
 * @param  read_hold_cb
 * @note null
 */
void modbus_server_add_read_hold_cb(modbus_bus_t *bus, modbus_read_hold_cb_t read_hold_cb) {
    bus->read_hold_cb = read_hold_cb;
}
/**
 * @brief 服务端添加读输入寄存器回调
 * @param  bus
 * @param  read_input_cb
 * @note null
 */
void modbus_server_add_read_input_cb(modbus_bus_t *bus, modbus_read_input_t read_input_cb) {
    bus->read_input_cb = read_input_cb;
}
/**
 * @brief 服务端添加写寄存器回调
 * @param  bus
 * @param  write_reg_cb
 * @note null
 */
void modbus_server_add_write_reg_cb(modbus_bus_t *bus, modbus_write_reg_cb_t write_reg_cb) {
    bus->write_reg_cb = write_reg_cb;
}

/**
 * @brief 解析modbus数据
 * @param  bus
 * @param  data             接收到的数据
 * @param  len              数据长度
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_anslysis(modbus_bus_t *bus, uint8_t *data, uint16_t len) {
    if (len < 4) {
        return -1; // 数据长度小于最小长度
    }
    if (bus->server_addr) { // 服务端
        // 不是自己的地址
        if (data[0] != bus->server_addr) {
            return -1;
        }
        // 校验crc
        uint16_t crc = crc_modbus(data, len - 2);
        if (crc != (data[len - 1] << 8 | data[len - 2])) {
            return -1;
        }
        uint16_t reg_addr = (data[2] << 8) | data[3];
        uint16_t reg_data = (data[4] << 8) | data[5];
        bus->data = data;
        bus->len = len;
        switch (data[1]) {
        case FUN_CODE_HOLD_REGS:
        case FUN_CODE_INPUT_REGS:
            modbus_server_analyze_03_04(bus, data[1], reg_addr, reg_data);
            break;
        case FUN_CODE_WRITE_REG:
            modbus_server_analyze_06(bus, reg_addr, reg_data);
            break;
        case FUN_CODE_WRITE_REGS:
            modbus_server_analyze_10(bus, reg_addr, reg_data);
            break;
        default:
            // 非法功能码
            break;
        }
    } else { // 客户端
        bus->data = data;
        bus->len = len;
    }
    return 0;
}

/**
 * @brief 设置modbus超时时间
 * @param  dev
 * @param  timeout
 * @note null
 */
void modbus_set_timeout(modbus_dev_t *dev, uint16_t timeout) {
    dev->timeout = timeout;
}
/**
 * @brief 设置modbus crc高低字节反转
 * @param  dev
 * @param  reversal 0低字节在前 1高字节在前
 * @note null
 */
void modbus_set_crc_reversal(modbus_dev_t *dev, uint8_t reversal) {
    dev->reversal = reversal;
}
/**
 * @brief 连续读取保持寄存器 03h
 * @param  dev
 * @param  reg_addr         寄存器起始地址
 * @param  reg_num          寄存器数量 max 125
 * @param  reg_data         读取的寄存器数据
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_read_holding_registers(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_num, uint16_t *reg_data) {
    return modbus_client_03_04_fun(dev, FUN_CODE_HOLD_REGS, reg_addr, reg_num, reg_data);
}
/**
 * @brief 连续读取输入寄存器 04h
 * @param  dev
 * @param  reg_addr         寄存器起始地址
 * @param  reg_num          寄存器数量 max 125
 * @param  reg_data         读取的寄存器数据
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_read_input_registers(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_num, uint16_t *reg_data) {
    return modbus_client_03_04_fun(dev, FUN_CODE_INPUT_REGS, reg_addr, reg_num, reg_data);
}

/**
 * @brief 写单个保持寄存器 06h
 * @param  dev
 * @param  reg_addr         寄存器地址
 * @param  reg_data         寄存器数据
 * @return int 0成功 -1失败
 * @note null
 */
int modbus_write_single_register(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_data) {
    if (dev->bus->server_addr) {
        dev->flag.err_usage = 1;
        return -1;
    }
    int ret = -1;
    uint8_t data[8];
    modbus_client_fill_head(data, dev->dev_addr, FUN_CODE_WRITE_REG, reg_addr, reg_data);
    uint16_t crc = crc_modbus(data, 6);
    if (dev->reversal) {
        crc = (crc >> 8) | (crc << 8);
    }
    data[6] = crc;
    data[7] = crc >> 8;
    if (modbus_send(dev, data, 8) < 0) {
        goto __exit;
    }
    if (modbus_verify(dev, FUN_CODE_WRITE_REG) < 0) {
        goto __exit;
    }
    ret = modbus_client_analyze_06(dev, reg_data);
__exit:
    return ret;
}
/**
 * @brief 写多个保持寄存器 10h
 * @param  dev
 * @param  reg_addr
 * @param  reg_num
 * @param  reg_data
 * @return int
 * @note null
 */
int modbus_write_multiple_registers(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_num, uint16_t *reg_data) {
    if (reg_num > 123 || dev->bus->server_addr) {
        dev->flag.err_usage = 1;
        return -1;
    }
    if ((reg_addr + reg_num) & 0x10000) {
        dev->flag.err_usage = 1;
        return -1;
    }
    int ret = -1;
    uint8_t data[9 + reg_num * 2];
    modbus_client_fill_head(data, dev->dev_addr, FUN_CODE_WRITE_REGS, reg_addr, reg_num);
    data[6] = reg_num * 2;
    for (size_t i = 0; i < reg_num; i++) {
        data[7 + i * 2] = reg_data[i] >> 8;
        data[8 + i * 2] = reg_data[i];
    }
    uint16_t crc = crc_modbus(data, 6 + reg_num * 2);
    if (dev->reversal) {
        crc = (crc >> 8) | (crc << 8);
    }
    data[7 + reg_num * 2] = crc;
    data[8 + reg_num * 2] = crc >> 8;
    if (modbus_send(dev, data, 9 + reg_num * 2) < 0) {
        goto __exit;
    }
    if (modbus_verify(dev, FUN_CODE_WRITE_REGS) < 0) {
        goto __exit;
    }
    ret = modbus_client_analyze_10(dev, reg_addr, reg_num);
__exit:
    return ret;
}

// 添加写保持寄存器的回调函数 返回0写入成功 返回-1寄存器只读



/**
 * @brief 获取错误信息
 * @param  modbus
 * @return const char*
 * @note null
 */
const char *modbus_get_err_info(modbus_dev_t *modbus) {
    if (modbus->flag.err_crc) {
        return "CRC error";
    }
    if (modbus->flag.err_addr) {
        return "Device address error";
    }
    if (modbus->flag.err_func) {
        return "Function code error";
    }
    if (modbus->flag.err_timeout) {
        return "Timeout";
    }
    if (modbus->flag.err_usage) {
        return "Usage error";
    }
    // 异常码 = 功能码+0x80 01非法功能 02非法数据地址 03非法数据值 04从站设备故障 05确认 06设备忙 08存储奇偶校验错误 0A网关路径不可用 0B网关目标设备响应超时 0C网关目标设备无响应
    if (modbus->exception_func) {
        switch (modbus->exception_code) {
        case 0x01:
            return "Illegal function"; // 非法功能码
        case 0x02:
            return "Illegal data address"; // 非法数据地址 寄存器地址不存在
        case 0x03:
            return "Illegal data value"; // 非法数据值
        case 0x04:
            return "Slave device failure"; // 从设备故障
        case 0x05:
            return "Acknowledge"; // 承认
        case 0x06:
            return "Device busy"; // 设备忙
        case 0x08:
            return "Storage parity error"; //
        case 0x0A:
            return "Gateway path unavailable";
        case 0x0B:
            return "Gateway target device response timeout";
        case 0x0C:
            return "Gateway target device no response";
        default:
            return "Unknown error";
        }
    }
    return "Unknown error";
}


/**********************************Private Functions*******************************/

static int modbus_client_analyze_03(modbus_dev_t *dev, uint16_t *reg_data, uint16_t reg_num) {
    modbus_bus_t *bus = dev->bus;
    // 寄存器数量
    uint16_t data_num = bus->data[2] / 2;
    if (data_num < reg_num) {
        for (size_t i = 0; i < data_num; i++) {
            reg_data[i] = (bus->data[3 + i * 2] << 8) | bus->data[4 + i * 2];
        }
    } else {
        for (size_t i = 0; i < reg_num; i++) {
            reg_data[i] = (bus->data[3 + i * 2] << 8) | bus->data[4 + i * 2];
        }
    }
    return 0;
}

static int modbus_client_analyze_06(modbus_dev_t *dev, uint16_t reg_data) {
    modbus_bus_t *bus = dev->bus;
    uint16_t ack_data = (bus->data[4] << 8) | bus->data[5];
    if (ack_data != reg_data) {
        return -1;
    }
    return 0;
}

static int modbus_client_analyze_10(modbus_dev_t *dev, uint16_t reg_addr, uint16_t reg_num) {
    modbus_bus_t *bus = dev->bus;
    uint16_t ack_addr = (bus->data[2] << 8) | bus->data[3];
    uint16_t ack_num = (bus->data[4] << 8) | bus->data[5];
    if ((ack_addr != reg_addr) || (ack_num != reg_num)) {
        return -1;
    }
    return 0;
}

static int modbus_server_analyze_03_04(modbus_bus_t *bus, uint8_t fun, uint16_t reg_addr, uint16_t reg_num) {
    uint8_t response[256]; // 应答缓冲区 最高125个寄存器 + 地址1 + 功能码1 + 字节数1 + crc16
    response[1] = bus->server_addr;
    response[2] = fun;
    if ((reg_addr + reg_num) & 0x10000) {
        // 非法地址
        response[3] = 0x02;
        goto __err;
    }
    if ((bus->read_hold_cb == NULL) && (fun == FUN_CODE_HOLD_REGS)) {
        // 没有读取寄存器回调函数
        response[3] = 0x01;
        goto __err;
    }
    if ((bus->read_input_cb == NULL) && (fun == FUN_CODE_INPUT_REGS)) {
        // 没有读取寄存器回调函数
        response[3] = 0x01;
        goto __err;
    }
    int (*read_cb)(uint16_t reg_addr, uint16_t *reg_data, void *user_data);
    switch (fun) {
    case FUN_CODE_HOLD_REGS:
        read_cb = bus->read_hold_cb;
        break;
    case FUN_CODE_INPUT_REGS:
        read_cb = bus->read_input_cb;
    default:
        return -1;
    }
    response[3] = 0;
    uint16_t *reg = (void *)&response[4];
    for (uint8_t i = 0; i < reg_num; i++) {
        if (read_cb(reg_addr + i, reg + i, bus->user_data) < 0) {
            break;
        }
        response[3] += 2;
    }
    if (response[3] != reg_num * 2) {
        // 返回非法地址
        response[3] = 0x02;
    __err:
        response[2] |= 0x80;
        uint16_t crc = crc_modbus(&response[1], 3);
        response[4] = crc;
        response[5] = crc >> 8;
        bus->ctrl_cb('w', bus->user_data_ctrl);
        bus->send_cb(&response[1], 5, bus->user_data);
        bus->ctrl_cb('r', bus->user_data_ctrl);
        return -1;
    }
    uint16_t crc = crc_modbus(&response[1], 3 + response[3]);
    response[4 + response[3]] = crc;
    response[5 + response[3]] = crc >> 8;
    bus->ctrl_cb('w', bus->user_data_ctrl);
    bus->send_cb(&response[1], 5 + response[3], bus->user_data);
    bus->ctrl_cb('r', bus->user_data_ctrl);
    return 0;
}

static int modbus_server_analyze_06(modbus_bus_t *bus, uint16_t reg_addr, uint16_t reg_data) {
    uint8_t response[8];
    response[0] = bus->server_addr;
    response[1] = FUN_CODE_WRITE_REG;
    if (bus->write_reg_cb == NULL) {
        response[2] = 0x01; // 没有写寄存器回调函数
        goto __err;
    }
    if (bus->write_reg_cb(reg_addr, reg_data, bus->user_data) < 0) {
        response[2] = 0x02; // 非法数据地址
    __err:
        response[1] |= 0x80; // 异常码
        uint16_t crc = crc_modbus(response, 3);
        response[3] = crc;
        response[4] = crc >> 8;
        bus->ctrl_cb('w', bus->user_data_ctrl);
        bus->send_cb(response, 5, bus->user_data);
        bus->ctrl_cb('r', bus->user_data_ctrl);
        return -1;
    }
    response[2] = reg_addr >> 8;
    response[3] = reg_addr;
    response[4] = reg_data >> 8;
    response[5] = reg_data;
    uint16_t crc = crc_modbus(response, 6);
    response[6] = crc;
    response[7] = crc >> 8;
    bus->ctrl_cb('w', bus->user_data_ctrl);
    bus->send_cb(response, 8, bus->user_data);
    bus->ctrl_cb('r', bus->user_data_ctrl);
    return 0;
}

static int modbus_server_analyze_10(modbus_bus_t *bus, uint16_t reg_addr, uint16_t reg_num) {
    uint8_t response[8];
    uint16_t crc;
    response[0] = bus->server_addr;
    response[1] = FUN_CODE_WRITE_REGS;
    if (bus->write_reg_cb == NULL) {
        response[2] = 0x01; // 没有写寄存器回调函数
        goto __err;
    }
    for (uint16_t i = 0; i < reg_num; i++) {
        if (bus->write_reg_cb(reg_addr + i, (bus->data[7 + i * 2] << 8) | bus->data[8 + i * 2], bus->user_data) < 0) {
            response[2] = 0x02; // 非法数据地址
            goto __err;
        }
    }
    response[2] = reg_addr >> 8;
    response[3] = reg_addr;
    response[4] = reg_num >> 8;
    response[5] = reg_num;
    crc = crc_modbus(response, 6);
    response[6] = crc;
    response[7] = crc >> 8;
    bus->ctrl_cb('w', bus->user_data_ctrl);
    bus->send_cb(response, 8, bus->user_data);
    bus->ctrl_cb('r', bus->user_data_ctrl);
    return 0;
__err:
    response[1] |= 0x80; // 异常码
    crc = crc_modbus(response, 3);
    response[3] = crc;
    response[4] = crc >> 8;
    bus->ctrl_cb('w', bus->user_data_ctrl);
    bus->send_cb(response, 5, bus->user_data);
    bus->ctrl_cb('r', bus->user_data_ctrl);
    return -1;
}

static int modbus_client_03_04_fun(modbus_dev_t *dev, uint8_t fun, uint16_t reg_addr, uint16_t reg_num, uint16_t *reg_data) {
    if (reg_num > 125 || dev->bus->server_addr) {
        dev->flag.err_usage = 1;
        return -1;
    }
    if ((reg_addr + reg_num) & 0x10000) {
        dev->flag.err_usage = 1;
        return -1;
    }

    int ret = -1;
    uint8_t data[8];
    modbus_client_fill_head(data, dev->dev_addr, fun, reg_addr, reg_num);
    uint16_t crc = crc_modbus(data, 6);
    if (dev->reversal) {
        crc = (crc >> 8) | (crc << 8);
    }
    data[6] = crc;
    data[7] = crc >> 8;
    if (modbus_send(dev, data, 8) < 0) {
        goto __exit;
    }
    if (modbus_verify(dev, fun) < 0) {
        goto __exit;
    }
    ret = modbus_client_analyze_03(dev, reg_data, reg_num);
__exit:
    return ret;
}

static void modbus_client_fill_head(uint8_t *data, uint8_t addr, uint8_t fun, uint16_t reg_addr, uint16_t reg_data) {
    data[0] = addr;
    data[1] = fun;
    data[2] = reg_addr >> 8;
    data[3] = reg_addr;
    data[4] = reg_data >> 8;
    data[5] = reg_data;
}

static int modbus_send(modbus_dev_t *dev, uint8_t *data, uint16_t len) {
    int ret = -1;
    modbus_bus_t *bus = dev->bus;
    modbus_lock(bus);
    modbus_clear_err(dev);
    if (bus->ctrl_cb) {
        bus->ctrl_cb('w', bus->user_data_ctrl);
    }
    bus->send_cb(data, len, bus->user_data);
    if (bus->ctrl_cb) {
        bus->ctrl_cb('r', bus->user_data_ctrl);
    }
    size_t i;
    for (i = 0; i < dev->timeout; i++) {
        DELAY_MS(1);
        if (bus->len) {
            break;
        }
    }
    if (i == dev->timeout) {
        dev->flag.err_timeout = 1;
        goto __exit;
    }
    ret = 0;
__exit:
    modbus_unlock(bus);
    return ret;
}

static int modbus_verify(modbus_dev_t *dev, uint8_t fun_code) {
    modbus_bus_t *bus = dev->bus;
    // 地址不匹配
    if (bus->data[0] != dev->dev_addr) {
        dev->flag.err_addr = 1;
        return -1;
    }
    if (bus->data[1] & 0x80) { // 异常码
        dev->exception_func = bus->data[1] & 0x7F;
        dev->exception_code = bus->data[2];
        return -1;
    }
    // 功能码不匹配
    if (bus->data[1] != fun_code) {
        dev->flag.err_func = 1;
        return -1;
    }
    // 校验crc
    uint16_t crc = crc_modbus(bus->data, bus->len - 2);
    if (dev->reversal) {
        crc = (crc >> 8) | (crc << 8);
    }
    if (crc != (bus->data[bus->len - 1] << 8 | bus->data[bus->len - 2])) {
        dev->flag.err_crc = 1;
        return -1;
    }
    return 0;
}

static void modbus_clear_err(modbus_dev_t *dev) {
    *((uint16_t *)&dev->flag) = 0;
    dev->exception_func = 0;
    dev->exception_code = 0;
    dev->bus->data = NULL;
    dev->bus->len = 0;
}

static int modbus_check_err(modbus_dev_t *dev) {
    if (*((uint16_t *)&dev->flag)) {
        return -1;
    }
    return 0;
}

static void modbus_lock(modbus_bus_t *bus) {
#if OS_USE == FREERTOS
    xSemaphoreTakeRecursive(bus->mutex, pdMS_TO_TICKS(LOCK_TIMEOUT));
#endif
}

static void modbus_unlock(modbus_bus_t *bus) {
#if OS_USE == FREERTOS
    xSemaphoreGiveRecursive(bus->mutex);
#endif
}


/* [] END OF FILE */
